/**
  ******************************************************************************
  * File Name          : motor.c
  * Description        : 电机控制的对外接口
                     ##### How to use this driver #####
  ==============================================================================
	>>使用Motor_SetCtrlMode设置电机的控制模式，支持开环、速度、位置控制模式
	>>5ms-10ms的中断调用Motor_Ctrl进行电机的闭环控制
	>>在三种控制模式下，使用不同的控制函数
			Motor_SetPwr控制PWM
			Motor_SetSpd控制速度
			Motor_SetPos控制位置
  ******************************************************************************
  */

#include "motor.h"
#include "motor_bsp_stm32.h"
#include "main.h"
#include "leg.h"

void Motor_SetCtrlMode(Motor_t *motor, Motor_CtrlMode mode)
{
	motor->ctrl_mode = mode;
}

void Motor_SetPwr(Motor_t *motor, float pwr)
{
	Motor_SetPower(&motor->bsp, pwr);
}

void Motor_SetSpd(Motor_t *motor, float spd)
{
	motor->spd_pid.ref = spd;
}
float jscope_diff;
float jscope_out;
void Motor_SetPos(Motor_t *motor, float pos)
{
	float new_ref = (pos+motor->bsp.abs_angle_offset) *819.2f*2.f;
//	float max_diff = 3000;
	//if(motor == &leg[LEG_INDEX_RF].m1) jscope_diff = new_ref - motor->pos_pid.ref;
//	if(new_ref - motor->pos_pid.ref > max_diff) motor->pos_pid.ref += max_diff;
//	else if(new_ref - motor->pos_pid.ref < -max_diff) motor->pos_pid.ref -= max_diff;
	motor->pos_pid.ref = new_ref;
//	motor->pos_pid.ref = 0;
	//- motor->bsp.abs_angle_offset;
	//if(motor == &leg[LEG_INDEX_RF].m1) jscope_out = motor->pos_pid.ref;
}

/**
	@brief	电机控制代码
	@note		在定时器中断中调用
*/
void Motor_Ctrl_Calc(Motor_t *motor)
{
	motor->pos_pid.fdb = motor->bsp.angle + motor->bsp.turns*8192 - motor->bsp.encoder_angle_offset;
	
	motor->spd_pid.fdb = motor->bsp.speed;
	switch(motor->ctrl_mode)
	{
		case MOTOR_CTRL_SPEED:
		{
			motor->spd_pid.Calc(&motor->spd_pid);
			Motor_SetPower(&motor->bsp, motor->spd_pid.output);
		}
		break;
		
		case MOTOR_CTRL_POS:
		{
			motor->pos_pid.Calc(&motor->pos_pid);

			Motor_SetSpd(motor, motor->pos_pid.output);

			motor->spd_pid.Calc(&motor->spd_pid);
			Motor_SetPower(&motor->bsp, motor->spd_pid.output);

		}
		break;
		
		default:
			break;
	}
}


